/***************************************************************************************************************************
* state_from_rotors.h
*
* Author: Wxt
*
* Update Time: 2021.6.11
*
* 主要功能：
*    本库函数主要用于连接rotors_simulator（lee_position）与drone_control两个功能包。
*           1、订阅rotors_simulator功能包发布的无人机状态量。状态量包括无人机状态、位置、角度、角速度（？）。
*               注1： 这里并没有订阅所有可以来自rotors_simulator的消息，如需其他消息，请参阅rotors_simulator，修改代码。
*               注2：代码中，参与运算的角度均是以rad为单位，但是涉及到显示时或者需要手动输入时均以deg为单位。
*
***************************************************************************************************************************/
#ifndef STATE_FROM_ROTORS_H
#define STATE_FROM_ROTORS_H

#include <ros/ros.h>
#include <ros/console.h>
#include <bitset>
#include <math_utils.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Imu.h>
#include <uav_drone_msgs/DroneState.h>
#include <uav_drone_msgs/AttitudeReference.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Float64.h>
#include <tf2_msgs/TFMessage.h>
#include <tf/transform_listener.h>

using namespace std;

class state_from_rotors
{
    public:
    //变量声明 
    uav_drone_msgs::DroneState _DroneState;
    string mav_name;
    state_from_rotors(void):
    state_nh("~")
    {
        state_nh.param<string>("mav_name", mav_name, "firefly");
        // 【订阅】无人机当前位置 坐标系:（rotors的坐标系还不能确定）
        position_sub = state_nh.subscribe(std::string("/"+mav_name+"/odometry_sensor1/position").c_str(), 10, &state_from_rotors::pos_cb,this);
        vel_sub = state_nh.subscribe(std::string("/"+mav_name+"/odometry_sensor1/odometry").c_str(), 10, &state_from_rotors::vel_cb,this);
        // 【订阅】无人机当前欧拉角 坐标系:ENU系（IMU输出频率可在rqt中查看）
        //  本话题来自飞控(通过Mavros功能包 /plugins/imu.cpp读取), 对应Mavlink消息为ATTITUDE (#30), 对应的飞控中的uORB消息为vehicle_attitude.msg
        imu_sub = state_nh.subscribe<sensor_msgs::Imu>("/"+mav_name+ "/imu", 100, &state_from_rotors::imu_cb,this); 
    }
    
    private:

        ros::NodeHandle state_nh;

        ros::Subscriber position_sub;
        ros::Subscriber vel_sub;
        ros::Subscriber imu_sub;

        void pos_cb(const geometry_msgs::PointStamped &msg)
        {
            _DroneState.position[0]  = msg.point.x;
            _DroneState.position[1]  = msg.point.y;
            _DroneState.position[2]  = msg.point.z;

            //rotors_simulator和PX4的区别是，没有气压计作为相对高度的采集，并且是纯粹的仿真形式
            //所以，直接取仿真中的传感器值作为气压计的绝对高度真值
            _DroneState.rel_alt = msg.point.z;
        }
        void vel_cb(const nav_msgs::Odometry::ConstPtr& msg)
        {
            _DroneState.velocity[0] = msg->twist.twist.linear.x;
            _DroneState.velocity[1] = msg->twist.twist.linear.y;
            _DroneState.velocity[2] = msg->twist.twist.linear.z;
        }
        void imu_cb(const sensor_msgs::Imu::ConstPtr& msg)
        {
            Eigen::Quaterniond q_fcu = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
            //Transform the Quaternion to euler Angles
            Eigen::Vector3d euler_fcu = quaternion_to_euler(q_fcu);
            
            _DroneState.attitude_q.w = q_fcu.w();
            _DroneState.attitude_q.x = q_fcu.x();
            _DroneState.attitude_q.y = q_fcu.y();
            _DroneState.attitude_q.z = q_fcu.z();
            //[0]:  yaw              [1]:   pitch                [2]:    roll
            _DroneState.attitude[0] = euler_fcu[0];
            _DroneState.attitude[1] = euler_fcu[1];
            _DroneState.attitude[2] = euler_fcu[2];

            _DroneState.attitude_rate[0] = msg->angular_velocity.x;
            _DroneState.attitude_rate[1] = msg->angular_velocity.y;
            _DroneState.attitude_rate[2] = msg->angular_velocity.z;

            // 没有加速度反馈，所以暂时不通过对速度求导数的方式获取加速度
            // 同样，没有偏航角速率的反馈（可能是msg->angular_velocity.z）
        }

};

    
#endif
